/**
*
* @file tlv.c
*
* @brief TLV creation/parsing utility function definitions
*
* These functions encode or decode TLV data structures, primarily ASN.1
* BER-coded messages.  They are used in conjunction with functions in
* asn1.c.  See that file for additional details.
*
* Copyright ?2011 HID Corporation.  All rights reserved.
*
* This software is protected by copyright law and international treaties.
* Any unauthorized reproduction, distribution or use of the software is prohibited.
*
*/

// Exceptions:
// Calling these functions may result in exceptions being thrown:

// TLV_INSUFFICIENT_BUFFER - insufficient buffer length for the item
//              being added.
// TLV_INVALID_SETLENGTH - item length and forcedLengthSize are incompatible
// TLV_MALFORMED - for message decoding functions:
//          tag was greater than two bytes
//          TLV length went beyond the end of the input buffer
//          TLV length incorrect for data byte (boolean)
//          the length of data exceeds the size of the receiving item,
//              such as specific integer length
//          integer data is negative for unsigned integer receiving item
//          BER indefinite length is used (not supported)
//
#include "exception.h"
#include "utils.h"
#include "asn1.h"
#include "tlv.h"


/// <summary>
/// Encode: Set the Tag of a TLV
///
/// This function writes the single-byte tag to the output buffer and updates
/// the buffer position and length.  No processing is done with the tag value.
/// </summary>
/// <param name="tag">Tag of the TLV (must be 1 byte long, ie long-form tag not supported)</param>
/// <param name="result">[in/out] input: pointer to pointer to next unused location in output buffer,
///                               output: pointer to pointer to next unused location in output buffer after length adjustment for closed TLV</param>
/// <param name="resultLength">[in/out] input: pointer to maximum number of remaining message bytes,
///                               output: pointer to maximum number of remaining message bytes after adding tag</param>
void tlv_setTag(uint8_t tag, uint8_t** result, uint16_t* resultLength)
{
    if (*resultLength < 1) {
        Throw(TLV_INSUFFICIENT_BUFFER);
    }

    (*result)[0] = tag;
    (*result)++;
    (*resultLength)--;
}

/// <summary>
/// Encode: Set the Length of a TLV
///
/// The function encodes the specified length into the message buffer at the next location
/// and advances the output pointer.  It normally follows encoding of a tag and precedes
/// coding of the value for a TLV item.
///
/// The result is DER compliant if forcedLengthSize == TLV_ANY_LENGTH_SIZE.
/// </summary>
/// <param name="length">length to encode</param>
/// <param name="result">[in/out] input: pointer to pointer to next unused location in output buffer,
///                               output: pointer to pointer to next unused location in output buffer after length adjustment for closed TLV</param>
/// <param name="resultLength">[in/out] input: pointer to maximum number of remaining message bytes,
///                               output: pointer to maximum number of remaining message bytes after adding length</param>
/// <param name="forcedLengthSize">the length of the encoded length or TLV_ANY_LENGTH_SIZE for minimum size (DER-compliant).</param>

void tlv_setLength(uint16_t length, uint8_t** result, uint16_t* resultLength, uint8_t forcedLengthSize)
{
    if ((length < 128) && ((forcedLengthSize == TLV_ANY_LENGTH_SIZE) || (forcedLengthSize == 1))) {
        if (*resultLength < 1) {
            Throw(TLV_INSUFFICIENT_BUFFER);
        }

        (*result)[0] = (uint8_t)length;

        (*result)++;
        (*resultLength)--;
    }
    else {
        if ((forcedLengthSize == TLV_ANY_LENGTH_SIZE) || (forcedLengthSize == 3)) {
            if (*resultLength < 3) {
				Throw(TLV_INSUFFICIENT_BUFFER);
            }

            (*result)[0] = 0x82;
            (*result)[1] = (uint8_t)(length >> 8);
            (*result)[2] = (uint8_t)length;

            (*result) += 3;
            (*resultLength) -= 3;
        } else {
			Throw(TLV_INVALID_SETLENGTH);
        }
    }
}

#if 0
void tlv_setLength(uint16_t length, uint8_t** result, uint16_t* resultLength, uint8_t forcedLengthSize)
{
    if ((length < 128) && ((forcedLengthSize == TLV_ANY_LENGTH_SIZE) || (forcedLengthSize == 1))) {
        if (*resultLength < 1) {
            Throw(TLV_INSUFFICIENT_BUFFER);
        }

        (*result)[0] = (uint8_t)length;

        (*result)++;
        (*resultLength)--;
    }
    else if ((length < 256) && ((forcedLengthSize == TLV_ANY_LENGTH_SIZE) || (forcedLengthSize == 2))) {
        if (*resultLength < 2) {
            Throw(TLV_INSUFFICIENT_BUFFER);
        }

        (*result)[0] = 0x81;
        (*result)[1] = (uint8_t)length;

        (*result) += 2;
        (*resultLength) -= 2;
    }
    else {
        if ((forcedLengthSize == TLV_ANY_LENGTH_SIZE) || (forcedLengthSize == 3)) {
            if (*resultLength < 3) {
				Throw(TLV_INSUFFICIENT_BUFFER);
            }

            (*result)[0] = 0x82;
            (*result)[1] = (uint8_t)(length >> 8);
            (*result)[2] = (uint8_t)length;

            (*result) += 3;
            (*resultLength) -= 3;
        } else {
			Throw(TLV_INVALID_SETLENGTH);
        }
    }
}
#endif 


/// <summary>
/// Encode: Append the provided data to the output message.
///
/// This is normally used for the data field of a TLV item.  It follows encoding of the associated tag and length.
/// </summary>
/// <param name="data">[in] array containing the TLV value</param>
/// <param name="dataLength">value length</param>
/// <param name="result">[in/out] input: pointer to pointer to next unused location in output buffer,
///                               output: pointer to pointer to next unused location in output buffer after adding data bytes</param>
/// <param name="resultLength">[in/out] input: pointer to maximum number of remaining message bytes,
///                               output: pointer to maximum number of remaining message bytes after a length adjustment for added bytes</param>
void tlv_setData(uint8_t* Data, uint16_t dataLength, uint8_t** result, uint16_t* resultLength)
{
    if (*resultLength < dataLength) {
		Throw(TLV_INSUFFICIENT_BUFFER);
    }

    utils_copy(Data, *result, dataLength);
    (*result) += dataLength;
    (*resultLength) -= dataLength;
}


/// <summary>
/// Encode: Append a TLV using the provided Tag & data
/// </summary>
/// <param name="tag">single-byte Tag</param>
/// <param name="data">[in] array containing the TLV value</param>
/// <param name="dataLength">the TLV value length</param>
/// <param name="result">[in/out] input: pointer to pointer to next unused location in output buffer,
///                               output: pointer to pointer to next unused location in output buffer after adding TLV</param>
/// <param name="resultLength">[in/out] input: pointer to maximum number of remaining message bytes,
///                               output: pointer to maximum number of remaining message bytes after adding TLV</param>
void tlv_create(uint8_t tag, uint8_t* Data, uint16_t dataLength, uint8_t** result, uint16_t* resultLength)
{
	if ((tag == ASN1_TAG_INTEGER) && (Data[0] > 0x7F)) {
		// we entertain an INTEGER type consistency check here to avoid duplicating it everywhere.
		Throw(TLV_INSUFFICIENT_BUFFER);
	}
    tlv_setTag(tag, result, resultLength);
    tlv_setLength(dataLength, result, resultLength, TLV_ANY_LENGTH_SIZE);
    tlv_setData(Data, dataLength, result, resultLength);
}


/// <summary>
/// Decode: Skip ahead in TLV message buffer.
///
/// The next byte pointer is advanced and remaining length reduced by skipLength.
/// The contents of the message is not used.  A TLV_INSUFFICIENT_BUFFER exception
/// is thrown if the pointer advances beyond the message.
/// </summary>
/// <param name="skipLength"> number of message bytes to skip</param>
/// <param name="result"> [in/out] in: pointer to pointer to next message byte,
///                                out: pointer to pointer to next message byte after skipping</param>
/// <param name="resultLength">[in/out] input: pointer to number of remaining message bytes,
///                                     output: pointer to number of remaining message bytes after skipping</param>
void tlv_skip(uint16_t skipLength, uint8_t** result, uint16_t* resultLength)
{
    if (*resultLength < skipLength) {
		Throw(TLV_INSUFFICIENT_BUFFER);
    }

    (*result) += skipLength;
    (*resultLength) -= skipLength;
}


/// <summary>
/// Decode: Return Length of TLV item.
///
/// A TLV_MALFORMED exception is thrown if
///  there are insufficient message bytes to complete the length encoding
///  the length exceeds the maximum possible return value
/// </summary>
/// <param name="data"> [in/out] in: pointer to pointer to the first length byte of a TLV item,
///                              out: pointer to pointer to first data byte of same TLV item</param>
/// <param name="length">[in/out] input: pointer to number of remaining message bytes,
///                               output: pointer to number of remaining message bytes extracting TLV length</param>
/// <returns> length of current TLV value field</returns>
uint16_t tlv_readLength(uint8_t** Data, uint16_t* length)
{
    uint16_t len;

    if (*length == 0) {
        Throw(TLV_MALFORMED);
    }

    len = **Data;
    (*Data)++;
    (*length)--;

    if ((len & ASN1_LENGTH_LONGFORM) == ASN1_LENGTH_LONGFORM) {
        // long form
        uint8_t count;
        uint8_t i;

        count = len & ASN1_LENGTH_MASK;
        len = 0;
        for (i = 0; i < count; i++) {

            if (*length == 0) {
                Throw(TLV_MALFORMED);
            }

            len = (len << 8) + **Data;
            (*Data)++;
            (*length)--;
        }
    }

    return len;
}


/// <summary>
/// Decode: Return Tag of TLV item
///
/// This routine returns the BER-formatted tag value from the TLV-stream and
/// advances the pointer to the length field.  This routine supports only one- and two-byte tags.
/// A NOT_SUPPORTED exception is thrown for BER tags longer than 2 bytes.
///
/// Note that the position of the class and PC bits in the return value vary,
/// depending on whether the tag is one or two bytes long.
/// </summary>
/// <param name="data"> [in/out] in: pointer to pointer to the first tag byte of a TLV item,
///                              out: pointer to pointer to first length byte of same TLV item</param>
/// <param name="length">[in/out] input: pointer to number of remaining message bytes,
///                               output: pointer to number of remaining message bytes extracting TLV tag</param>
/// <returns> tag of current TLV: single byte = tag byte image,
///             double byte = (firstByte << 8) | secondByte.
/// </returns>
uint16_t tlv_readTag(uint8_t** Data, uint16_t* length)
{
    uint16_t tag;

    if (*length == 0) {
        Throw(TLV_NOT_FOUND);
    }

	// we assume usual BER TLV
	if ((**Data == 0) || (**Data == 0xFF)) {
        Throw(NOT_SUPPORTED);
	}

    tag = **Data;
    (*Data)++;
    (*length)--;

    // multi-byte tag
    if ((tag & ASN1_TAG_LONGFORM) == ASN1_TAG_LONGFORM) {
		// we support only 1- or 2-byte tags
		if ((**Data & 0x80) == 0x80) {
        	Throw(NOT_SUPPORTED);
		}

        if (*length == 0) {
            Throw(TLV_MALFORMED);
        }

        tag = (tag << 8) + **Data;
        (*Data)++;
        (*length)--;
    }

    return tag;
}


/// <summary>
/// Decode: Non-recursive TLV search
///
/// Search for the first matching tag.  This searches within all constructed types.
/// It requires use of BER constructed bit within tags.
///
/// Notes:
///   If the selected tag is ASN1_TAG_INTEGER, then a TLV_MALFORMED exception
///   is thrown if the value is negative.
///   If the specified tag is not found, a TLV_NOT_FOUND exception is thrown.
///   If searchTag == TLV_ANY_TAG, the tag is not returned (it can be obtained
///   by calling tlv_readTag() with the original data pointer).
///   The invariant (*data + *length == byte after buffer) is NOT true when this
///   function returns, unlike other encoding and decoding routines.  To restore
///   this condition, execute *data += returnValue.
/// </summary>
/// <remarks> non-recursive: avoid stack overflow </remarks>
/// <param name="data"> [in/out] in: pointer to pointer to first tag byte of first item in TLV structure to search,
///                              out: pointer to pointer to first data field byte of the matching tag</param>
/// <param name="length"> [in/out] input: pointer to number of message bytes to be searched,
////                               output: pointer to remaining bytes in original count, excluding
////                               all TLV bytes of the matching item and all prior TLVs which were skipped</param>
/// <param name="searchTag"> tag to search for, including class and constructed bits.
///                          For two-byte tags, the value is ((firstByte << 8) | secondByte).
/// <returns> object length </returns>
uint16_t tlv_search(uint8_t** Data, uint16_t* length, uint16_t searchTag)
{
    uint16_t tag;
    uint16_t objectLen;

again:;
    tag = tlv_readTag(Data, length);
    objectLen = tlv_readLength(Data, length);

    if (*length < objectLen) {
        Throw(TLV_MALFORMED);
    }

    if ((searchTag == TLV_ANY_TAG) || (tag == searchTag)) {
		// we entertain an INTEGER type consistency check here to avoid duplicating it everywhere.
		if ((tag == ASN1_TAG_INTEGER) && (*Data[0] > 0x7F)) {
			Throw(TLV_MALFORMED);
		}
		// set length to current TLV length.
        // data is already pointing onto the value part of the TLV.
        (*length) -= objectLen;
        return objectLen;
    } else {

        // we already point onto the value part (children)

        // focus on tag msb (contains primitive vs constructed flag)
        if (tag > 0xFF) {
            // tag is reloaded on the next iteration, so we can corrupt it safely.
            tag >>= 8;
        }

        if ((tag & ASN1_CONSTRUCTED) == 0) {
            // this is a primitive object, hence we skip its content
            (*Data) += objectLen;
            (*length) -= objectLen;
        }

        goto again;
    }
}
